#include "jerasure.h"
#include "galois.h"

/* Creates an empty matrix. */
JER_Matrix::JER_Matrix()
{
}

/* Creates an empty matrix. Sets r, c, w, and resizes Elts. */
JER_Matrix::JER_Matrix( int r, int c, int w )
{
	this->R = r;
	this->C = c;
	this->W = w;

	Resize_Elts();
}

/* Sets values of previously allocated matrix to all 0s. */
int JER_Matrix::Create_Empty( int r, int c, int w )
{
	Elts.clear();
	this->R = r;
	this->C = c;
	this->W = w;

	return Resize_Elts();
}
/* Sets values of previously allocated matrix to that of an identity. */
int JER_Matrix::Create_Identity( int r, int c, int w )
{
	int i;

	if( r != c || Create_Empty( r, c, w ) )
		return -1;
	for( i = 0; i < r; i++ )
		Set( i, i, 1 );
}

/* Sets the r,c element of matrix to val. */
void JER_Matrix::Set( int r, int c, uint64_t val )
{
	char mask;
	char* tmp;

	switch( W )
	{
		case 1:
			tmp = (char*) &(Elts[(r*C+c)/64]);
			tmp += ((r*C+c)%64)/8;
			mask = -1;

			if( val )
				*tmp = *tmp | (1 << (r*C+c)%8);
			else
				*tmp = *tmp & ( mask ^ (1 << (r*C+c)%8) );
			break;
		case 2:
		case 3:
		case 4:
		case 5:
		case 6:
		case 7:
		case 8:
			tmp = (char*) &(Elts[(r*C+c)/8]);
			tmp += (r*C+c)%8;
			*tmp = val;
			break;
		case 9:
		case 10:
		case 11:
		case 12:
		case 13:
		case 14:
		case 15:
		case 16:
			tmp = (char*) &(Elts[(r*C+c)/4]);
			tmp += ((r*C+c)%4)*2;
			*((uint16_t*)tmp) = val;
			break;
		case 17:
		case 18:
		case 19:
		case 20:
		case 21:
		case 22:
		case 23:
		case 24:
		case 25:
		case 26:
		case 27:
		case 28:
		case 29:
		case 30:
		case 31:
		case 32:
			tmp = (char*) &(Elts[(r*C+c)/2]);
			tmp += ((r*C+c)%2)*4;
			*((uint32_t*)tmp) = val;
			break;
		default:
			Elts[r*C+c] = val;
	}
}

/* Gets the value of r,c element of matrix. */
uint64_t JER_Matrix::Get( int r, int c )
{
	uint64_t* tmp;

	switch( W )
	{
		case 1:
			tmp = &(Elts[(r*C+c)/64]);

			return ( *tmp >> (r*C+c)%64 ) & 1;
			break;
		case 2:
		case 3:
		case 4:
		case 5:
		case 6:
		case 7:
		case 8:
			tmp = &(Elts[(r*C+c)/8]);

			return *((unsigned char *)tmp + ((r*C+c)%8));
			break;
		case 9:
		case 10:
		case 11:
		case 12:
		case 13:
		case 14:
		case 15:
		case 16:
			tmp = &(Elts[(r*C+c)/4]);
			return *((uint16_t*)tmp + ((r*C+c)%4));
			break;
		case 17:
		case 18:
		case 19:
		case 20:
		case 21:
		case 22:
		case 23:
		case 24:
		case 25:
		case 26:
		case 27:
		case 28:
		case 29:
		case 30:
		case 31:
		case 32:
			tmp = &(Elts[(r*C+c)/2]);
			return *((uint32_t*)tmp + ((r*C+c)%2));
		default:
			return Elts[r*C+c];
	}
}

/* Prints the contents of the matrix to stdout. */
//WPD is optional and defaults to 1
void JER_Matrix::Print(int WPD)
{
	int i, j;
	int fw;
	char s[30];
	uint64_t w2;
	int rows = Elts.size();
	int cols;

	if (W == 32) {
		fw = 10;
	} else {
		w2 = (1 << W);
		sprintf(s, "%llu", w2-1);
		fw = strlen(s);
	}

	for( i = 0; i < R; i++ )
	{
		//insert a new line separating super-rows
		if (WPD != 1 && i%WPD == 0 && i != 0){
			printf("\n");
		}
		for( j = 0; j < C; j++ )
		{
			//print spaces between super columns
			if (j != 0 && j%WPD == 0) printf(" ");
			printf("%*llu",fw, Get( i, j ) );
		}
		printf("\n");
	}

}

/* Returns a string of the contents of the matrix. */
string JER_Matrix::String(int WPD)
{
	string retstring;
	int i, j;

	retstring = "";

	for( i = 0; i < R; i++ )
	{
		if( WPD != 1 && i%WPD == 0 && i != 0 )
			retstring += "\n";
		for( j = 0; j < C; j++ )
			retstring += Get( i, j ) + " ";
		retstring += "\n";
	}
	return retstring;
}

/* Overwrites values of matrix with values of m. */
int JER_Matrix::Copy( JER_Matrix *m )
{
	R = m->R;
	C = m->C;
	W = m->W;
	try{ Elts = m->Elts; }
	catch(...){ return -1; }
	return 0;
}

/* XORs values of matrix with values of m. */
int JER_Matrix::Add( JER_Matrix *m )
{
	int i, j;

	if( R != m->R || C != m->C || W != m->W )
		return -1;
	for( i = 0; i < Elts.size(); i++ )
		Elts[i] ^= m->Elts[i];
	return 0;
}

/* Appends the rth row or cth column of m to the matrix. */
int JER_Matrix::Append_Row( JER_Matrix *m, int r )
{
	int j;

	if( C != m->C || W != m->W )
		return -1;
	R++;
	//Elts.resize( ((R+1)*C - 1) / elperint64 + 1, 0 );
	if( Resize_Elts() )
		return -1;

	for( j = 0; j < C; j++ )
		Set( R-1, j, m->Get( r, j ) );
}

int JER_Matrix::Append_Col( JER_Matrix *m, int c )
{
	int i, j;

	if( R != m->R || W != m->W )
		return -1;
	//Elts.resize( (R*(C+1) - 1) / elperint64 + 1, 0 );
	C++;
	if( Resize_Elts() )
		return -1;

	for( i = R-1; i > 0; i-- )
		for( j = C-2; j >= 0; j-- )
			Set( i, j, Get( i, j-i ) );

	for( i = 0; i < R; i++ )
		Set( i, C-1, m->Get( i, c ) );
}

/* Deletes the rth row or cth column of the matrix. Resizes matrix. 
   Does not resize Elts. */
void JER_Matrix::Delete_Row( int r )
{
	int i, j;

	//add this check. without it, deleting rows with indices > 2 on 
	//a 3 row matrix would delete the last row
	if(r >= R) return;

	R--;

	for( i = r; i < R; i++ )
		for( j = 0; j < C; j++ )
			Set( i, j, Get( i + 1, j ) );

	Resize_Elts();
}

void JER_Matrix::Delete_Col( int c )
{
	int i, j;

	if(c >= C) return;

	for( i = 0; i < R; i++ )
		for( j = 0; j < C; j++ )
		{
			if( j != c )
			{
				Set( i, j - i, Get( i, j ) );
				if( j > c )
					Set( i, j - i - 1, Get( i, j ) );
			}
		}
	C--;
	Resize_Elts();
}

/* Deletes nr rows or nc columns starting with starting_r/c. */
void JER_Matrix::Delete_Rows( int starting_r, int nr )
{
	int i;
	for( i = 0; i < nr; i++ )
		Delete_Row( starting_r );
}

void JER_Matrix::Delete_Cols( int starting_c, int nc )
{
	int i;
	for( i = 0; i < nc; i++ )
		Delete_Col( starting_c );
}

/* Copies values from m into matrix starting at element dest_r,dest_c.
   If src_r, src_c specified, only copies a submatrix of m. */

//private method
int JER_Matrix::copy_panel(JER_Matrix *m, int src_r, int src_c, int dest_r, int dest_c,int nr,int nc){

	int i, j;
	//variables for fast get/set
	uint64_t *elt_get,*elt_set;
	uint64_t mask_get,mask_set;
	uint64_t new_mask;
	char shift_get,shift_set;
	char bits_per_num, num_per_elt;
	int r,c;
	uint64_t tmp64;
	int cols_get,cols_set;

	if( R < dest_r + nr || C < dest_c + nc ||
			m->R < src_r + nr || m->C < src_c + nc || W != m->W ){
		return -1;
	}

	cols_get = m->C;
	cols_set = C;
	switch(W){
		case 1:
			bits_per_num = 1;
			break;
		case 2:
		case 3:
		case 4:
		case 5:
		case 6:
		case 7:
		case 8:
			bits_per_num = 8;
			break;
		case 9:
		case 10:
		case 11:
		case 12:
		case 13:
		case 14:
		case 15:
		case 16:
			bits_per_num = 16;
			break;
		case 17:
		case 18:
		case 19:
		case 20:
		case 21:
		case 22:
		case 23:
		case 24:
		case 25:
		case 26:
		case 27:
		case 28:
		case 29:
		case 30:
		case 31:
		case 32:
			bits_per_num = 32;
			break;
		default:
			return -1;
	}
	new_mask = ((uint64_t)1<<bits_per_num)-1;
	num_per_elt = 64/bits_per_num;

	for( i = 0; i < nr; i++ ){

		r=dest_r + i;
		c=dest_c;
		//ptr and bit-mask to set
		elt_set = &(Elts[(r*cols_set+c)/num_per_elt]);
		shift_set = bits_per_num*((r*cols_set+c)%num_per_elt);
		mask_set = new_mask << shift_set;

		//ptr and bit-bitmask to get
		r = src_r+i;
		c = src_c;
		elt_get = &(m->Elts[(r*cols_get+c)/num_per_elt]);
		shift_get = bits_per_num*((r*cols_get+c)%num_per_elt);
		mask_get = new_mask << shift_get;

		for( j = 0; j < nc; j++ ){

			tmp64 = ((*elt_get)&mask_get) >> shift_get; 

			//set element i,j
			//set jm_cpy
			//first set previous value to 0 
			(*elt_set) &= (~mask_set);
			//now set the new value
			(*elt_set) ^= (tmp64<<shift_set);

			mask_get <<= bits_per_num;
			shift_get += bits_per_num;
			if(mask_get==0){
				shift_get = 0;
				mask_get = new_mask;
				elt_get++;
			}

			mask_set <<= bits_per_num;
			shift_set += bits_per_num;
			if(mask_set==0){
				shift_set = 0;
				mask_set = new_mask;
				elt_set++;
			}
			
			//Set( dest_r + i, dest_c + j, m->Get( src_r + i, src_c + j ) );
			
		}
	}

	return 0;

}

int JER_Matrix::Copy_Panel( JER_Matrix *m, int dest_r, int dest_c )
{

	return copy_panel(m,0,0,dest_r,dest_c,m->R,m->C);

}

int JER_Matrix::Copy_Panel( JER_Matrix *m, int src_r, int src_c, int dest_r, 
		int dest_c, int nr, int nc ){

	return copy_panel(m,src_r,src_c,dest_r,dest_c,nr,nc);
}

/* XORs values from m into matrix starting at element dest_r,dest_c.
   If src_r, src_c specified, only copies a submatrix of m. */
int JER_Matrix::Add_Panel( JER_Matrix *m, int dest_r, int dest_c )
{
	int i, j;

	if( R < m->R + dest_r || C < m->C + dest_c )
		return -1;

	for( i = 0; i < m->R; i++ )
		for( j = 0; j < m->C; j++ )
			Set( dest_r + i, dest_c + j, 
				 Get( dest_r + i, dest_c + j ) ^ m->Get( i, j ) );
	return 0;
}

int JER_Matrix::Add_Panel( JER_Matrix *m, int src_r, int src_c, int dest_r, 
		int dest_c, int nr, int nc )
{
	int i, j;

	if( R < dest_r + nr || C < dest_c + nc ||
			m->R < src_r + nr || m->C < src_c + nc )
		return -1;

	for( i = 0; i < nr; i++ )
		for( j = 0; j < nc; j++ )
			Set( dest_r + i, dest_c + j, Get( dest_r + i, dest_c + j ) ^ 
					m->Get( src_r + i, src_c + j ) );
	return 0;
}

/* Cycles the columns/rows of the matrix by cols/rows. */
/* Positive cols shifts left by cols. */
void JER_Matrix::Horizontal_Rotate( int cols )
{
	uint64_t tmp;
	int i, j, k;

	if( cols > 0 )
		for( k = 0; k < cols; k++ )
			for( i = 0; i < R; i++ )
			{
				tmp = Get( i, 0 );
				for( j = 1; j < C; j++ )
					Set( i, j - 1, Get( i , j ) );
				Set( i, C - 1, tmp );
			}
	else
		for( k = cols; k < 0; k++ )
			for( i = 0; i < R; i++ )
			{
				for( j = C-1; j > 0; j-- )
					Set( i, j, Get( i , j - 1 ) );
				Set( i, 0, tmp );
			}
}

/* Positive rows shifts up by rows. */
void JER_Matrix::Vertical_Rotate( int rows )
{
	uint64_t tmp;
	int i, j, k;

	if( rows > 0 )
		for( k = 0; k < rows; k++ )
			for( j = 0; j < C; j++ )
			{
				tmp = Get( 0, j );
				for( i = 1; i < R; i++ )
					Set( i - 1, j, Get( i , j ) );
				Set( R - 1, 0, tmp );
			}
	else
		for( k = rows; k < 0; k++ )
			for( j = 0; j < C; j++ )
			{
				tmp = Get( R-1, j );
				for( i = R-1; i > 0; j-- )
					Set( i, j, Get( i - 1, j ) );
				Set( 0, j, tmp );
			}
}

/* Swaps the values in row r1 with those in row r2. Similar with cols. */
void JER_Matrix::Swap_Rows( int r1, int r2 )
{
	uint64_t tmp;
	int j;

	for( j = 0; j < C; j++ )
	{
		tmp = Get( r1, j );
		Set( r1, j, Get( r2, j ) );
		Set( r2, j, tmp );
	}
}

void JER_Matrix::Swap_Cols( int c1, int c2 )
{
	uint64_t tmp;
	int i;

	for( i = 0; i < R; i++ )
	{
		tmp = Get( i, c1 );
		Set( i, c1, Get( i, c2 ) );
		Set( i, c2, tmp );
	}
}

/* Row r1 += row r2. Similar for col. */
void JER_Matrix::Row_PlusEquals( int r1, int r2 )
{
	int i;

	for( i = 0; i < C; i++ )
		Set( r1, i, Get(r1,i) ^ Get(r2,i) );
}
void JER_Matrix::Col_PlusEquals( int c1, int c2 )
{
	int j;

	for( j = 0; j < R; j++ )
		Set( j, c1, Get(j,c1) ^ Get(j,c2) );
}

/* Row r1 += (row r2 * prod ). Similar for col. */
void JER_Matrix::Row_PlusEquals_Prod( int r1, int r2, int prod )
{
	int i;

	if( W == 1 )
		for( i = 0; i < C; i++ )
			Set( r1, i, Get(r1,i) ^ ( Get(r2,i) & prod ) );
	else
		for( i = 0; i < C; i++ )
			Set( r1, i, Get(r1,i) ^ ( galois_single_multiply( Get(r2,i), prod, W ) ) );
}
void JER_Matrix::Col_PlusEquals_Prod( int c1, int c2, int prod )
{
	int j;

	if( W == 1 )
		for( j = 0; j < R; j++ )
			Set( j, c1, Get(j,c1) ^ ( Get(j,c2) & prod ) );
	else
		for( j = 0; j < R; j++ )
			Set( j, c1, Get(j,c1) ^ ( galois_single_multiply( Get(j,c2), prod, W ) ) );
}

/* Row r1 *= prod. Similar for col. */
void JER_Matrix::Row_TimesEquals( int r1, int prod )
{
	int i;

	if( W == 1 )
		for( i = 0; i < C; i++ )
			Set( r1, i, Get(r1,i) & prod );
	else
		for( i = 0; i < C; i++ )
			Set( r1, i, galois_single_multiply( Get(r1,i), prod, W ) );
}
void JER_Matrix::Col_TimesEquals( int c1, int prod )
{
	int j;

	if( W == 1 )
		for( j = 0; j < R; j++ )
			Set( j, c1, Get(j,c1) & prod );
	else
		for( j = 0; j < R; j++ )
			Set( j, c1, galois_single_multiply( Get(j,c1), prod, W ) );
}

/* Adds m1 to m2 and returns it or stores in sum if specified. */
JER_Matrix *Sum( JER_Matrix *m1, JER_Matrix *m2 )
{
	JER_Matrix *result;

	result = new JER_Matrix();
	if( result == NULL )
		return NULL;

	if( result->Copy( m1 ) )
	{
		delete result;
		return NULL;
	}
	if( result->Add( m2 ) )
	{
		delete result;
		return NULL;
	}

	return result;
}
JER_Matrix Sum( JER_Matrix &m1, JER_Matrix &m2 )
{
	JER_Matrix result;

	if( m1.R == m2.R && m1.C == m2.C && m1.W == m2.W )
	{
		result.Copy( &m1 );
		result.Add( &m2 );
	}else{
		result.R = 0;
		result.C = 0;
		result.Elts.clear();
	}
	return result;
}
int Sum(JER_Matrix *m1, JER_Matrix *m2, JER_Matrix *sum)
{
	if( sum->Copy( m1 ) )
		return -1;
	if( sum->Add( m2 ) )
		return -1;
	return 0;
}
int Sum(JER_Matrix &m1, JER_Matrix &m2, JER_Matrix &sum)
{
	if( sum.Copy( &m1 ) )
		return -1;
	if( sum.Add( &m2 ) )
		return -1;
	return 0;
}

/* Multiplies m1 to m2 and returns it or stores in prod if specified. */
JER_Matrix *Prod( JER_Matrix *m1, JER_Matrix *m2 )
{
	JER_Matrix *result;
	int i, j, k;

	if( m1->C != m2->R || m1->W != m2->W )
		return NULL;

	result = new JER_Matrix( m1->R, m2->C, m1->W );
	if( result == NULL )
		return NULL;

	if( m1->W == 1 )
		for( i = 0; i < m1->R; i++ )
			for( j = 0; j < m1->C; j++ )
				for( k = 0; k < m2->C; k++ )
					result->Set( i, k, result->Get( i, k ) ^ 
							   ( m1->Get(i,j) & m2->Get(j,k) ) );
	else
		for( i = 0; i < m1->R; i++ )
			for( j = 0; j < m1->C; j++ )
				for( k = 0; k < m2->C; k++ )
					result->Set( i, k, result->Get( i, k ) ^ 
							galois_single_multiply( 
								m1->Get(i,j), m2->Get(j,k), m1->W ) );

	return result;
}
JER_Matrix Prod( JER_Matrix &m1, JER_Matrix &m2 )
{
	JER_Matrix result;
	int i, j, k;

	if( m1.C == m2.R && m1.W == m2.W )
	{
		result.Create_Empty( m1.R, m2.C, m1.W );

		if( m1.W == 1 )
			for( i = 0; i < m1.R; i++ )
				for( j = 0; j < m1.C; j++ )
					for( k = 0; k < m2.C; k++ )
						result.Set( i, k, result.Get( i, k ) ^ 
								  ( m1.Get(i,j) & m2.Get(j,k) ) );
		else
			for( i = 0; i < m1.R; i++ )
				for( j = 0; j < m1.C; j++ )
					for( k = 0; k < m2.C; k++ )
						result.Set( i, k, result.Get( i, k ) ^ 
								galois_single_multiply( 
									m1.Get(i,j), m2.Get(j,k), m1.W ) );
	}
	return result;
}

int Prod(JER_Matrix *m1, JER_Matrix *m2, JER_Matrix *prod)
{
	int i, j, k;

	if( m1->C != m2->R || m1->W != m2->W ||
			prod->Create_Empty( m1->R, m2->C, m1->W ) )
		return -1;
	if( m1->W == 1 )
		for( i = 0; i < m1->R; i++ )
			for( j = 0; j < m1->C; j++ )
				for( k = 0; k < m2->C; k++ )
					prod->Set( i, k, prod->Get( i, k ) ^ 
							 ( m1->Get(i,j) & m2->Get(j,k) ) );
	else
		for( i = 0; i < m1->R; i++ )
			for( j = 0; j < m1->C; j++ )
				for( k = 0; k < m2->C; k++ )
					prod->Set( i, k, prod->Get( i, k ) ^ 
							galois_single_multiply( 
								m1->Get(i,j), m2->Get(j,k), m1->W ) );
	return 0;
}
int Prod(JER_Matrix &m1, JER_Matrix &m2, JER_Matrix &prod)
{
	int i, j, k;

	if( m1.C != m2.R || m1.W != m2.W ||
			prod.Create_Empty( m1.R, m2.C, m1.W ) )
		return -1;
	if( m1.W == 1 )
		for( i = 0; i < m1.R; i++ )
			for( j = 0; j < m1.C; j++ )
				for( k = 0; k < m2.C; k++ )
					prod.Set( i, k, prod.Get( i, k ) ^ 
							( m1.Get(i,j) & m2.Get(j,k) ) );
	else
		for( i = 0; i < m1.R; i++ )
			for( j = 0; j < m1.C; j++ )
				for( k = 0; k < m2.C; k++ )
					prod.Set( i, k, prod.Get( i, k ) ^ 
							galois_single_multiply( 
								m1.Get(i,j), m2.Get(j,k), m1.W ) );
	return 0;
}

/*
The next two functions not declared in the .h file. They are the same 
except the matrix version calls galois single mult/div. 
Both matrices should be previously allocated, and inv is modified to be the
inverse of m. If m is not invertible, -1 is returned. 0 is 
returned on success. Before returning -1, inv is resized to 0x0.
*/
int invert_matrix(JER_Matrix *jm, JER_Matrix *jm_inv){

	JER_Matrix jm_cpy;
	int i, j, k;
	int rows, cols, w;

	//variables for fast get/set
	uint64_t *elt, *elt2, *elt3, *elt4;
	uint64_t mask;
	uint64_t new_elt_mask; 
	char shift,shift2;
	char bits_per_num, num_per_elt;
	int r,c;
	uint64_t *saved_elt; //saved elt and mask are used when we get the same r,c element multiple times
	uint64_t saved_mask;
	uint64_t tmp64_t,tmp64_t2,tmp64_t3;
	uint64_t mask32; //64 bit mask where the first 32 bits are 1's

	if(jm_inv == NULL){
		return -1;
	}

	if(jm == NULL){
		jm_inv->Elts.clear();
		jm_inv->R = 0;
		jm_inv->C = 0;
		return -1;	
	}

	rows = jm->R;
	cols = jm->C;
	w = jm->W;

	if( rows <= 0 || rows != cols){
		jm_inv->Elts.clear();
		jm_inv->R = 0;
		jm_inv->C = 0;
		return -1;	
	}

	//setup variables needed to Get elements from the padded Elts vector
	switch(w){
		case 1:
			bits_per_num = 1;
			break;
		case 2:
		case 3:
		case 4:
		case 5:
		case 6:
		case 7:
		case 8:
			bits_per_num = 8;
			break;
		case 9:
		case 10:
		case 11:
		case 12:
		case 13:
		case 14:
		case 15:
		case 16:
			bits_per_num = 16;
			break;
		case 17:
		case 18:
		case 19:
		case 20:
		case 21:
		case 22:
		case 23:
		case 24:
		case 25:
		case 26:
		case 27:
		case 28:
		case 29:
		case 30:
		case 31:
		case 32:
			bits_per_num = 32;
			break;
		default:
			jm_inv->Elts.clear();
			jm_inv->R = 0;
			jm_inv->C = 0;
			return -1;
	}
	new_elt_mask = ((uint64_t)1<<bits_per_num)-1;
	num_per_elt = 64/bits_per_num;
	mask32 = ((uint64_t)1<<32)-1;

	//copy this matrix
	jm_cpy.Copy(jm);

	//turn this matrix into an identity matrix 
	jm_inv->Create_Identity(rows,cols,w);

	for( i = 0; i < rows; i++ ){
		/* If there is a 0 on the diagonal on the ith row */
		r=i;
		c=i;
		saved_elt = &(jm_cpy.Elts[(r*cols+c)/num_per_elt]);
		shift = bits_per_num*((r*cols+c)%num_per_elt);
		saved_mask = new_elt_mask << shift;
		if( ((*saved_elt)&saved_mask) == 0 ){
		//if( jm_cpy.Get(i,i) == 0 )
			/* Go through each subsequent row looking for one with a non-zero
			   value in the ith column. When this row is moved to the ith row,
			   there will no longer be a 0 on the diagonal on the ith row. */
			j = i;
			do{
				j++;
				r=j;
				elt = &(jm_cpy.Elts[(r*cols+c)/num_per_elt]);
				mask = new_elt_mask << (bits_per_num*((r*cols+c)%num_per_elt));
			}while(j< rows && ((*elt)&mask) == 0);
			//for( j = i + 1; j < rows && jm_cpy.Get(j,i) == 0; j++ );

			/* If j == rows, there was no row with a non-zero value in the ith
				 column below row i, therefore matrix is not invertible. */
			if( j == rows){
				jm_inv->Elts.clear();
				jm_inv->R = 0;
				jm_inv->C = 0;
				return -1;
			}
			/* Otherwise swap the ith and jth row. */
			jm_cpy.Swap_Rows(i,j);
			jm_inv->Swap_Rows(i,j);
		}

		/* If the value on the diagonal is not 1, it must be reduced to 1 by 
		   dividing the row by the value on the diagonal. */

		//Get the exact same element that we got earlier (i,i)
		tmp64_t = ((*saved_elt)&saved_mask);
		tmp64_t >>= shift; 

		//tmp64_t = jm_cpy.Get(i,i);
		if( tmp64_t != 1 ){
			tmp64_t = mask32 & galois_single_divide( 1, tmp64_t, w);
			r=i;
			c=0;
			elt = &(jm_cpy.Elts[(r*cols+c)/num_per_elt]);
			elt2 = &(jm_inv->Elts[(r*cols+c)/num_per_elt]);
			shift = bits_per_num*((r*cols+c)%num_per_elt);
			mask = new_elt_mask << shift;
			for( j = 0; j < cols; j++ ){
				tmp64_t2 = ((*elt)&mask) >> shift; 
				tmp64_t3 = ((*elt2)&mask) >> shift; 
				
				tmp64_t2 = mask32 & galois_single_multiply(tmp64_t2, tmp64_t, w);
				tmp64_t3 = mask32 & galois_single_multiply(tmp64_t3, tmp64_t, w);
				
				//set element i,j
				//set jm_cpy
				//first set previous value to 0 
				(*elt) &= (~mask);
				//now set the new value
				(*elt) ^= (tmp64_t2<<shift);

				//set jm_inv
				//first set previous value to 0 
				(*elt2) &= (~mask);
				//now set the new value
				(*elt2) ^= (tmp64_t3<<shift);

				//setup to get the next number
				mask <<= bits_per_num;
				shift += bits_per_num;
				if(mask==0){
					shift = 0;
					mask = new_elt_mask;
					elt++;
					elt2++;
				}
				/*
				jm_cpy.Set(i,j,galois_single_multiply( jm_cpy.Get(i,j), tmp64_t, w));
				jm_inv->Set(i,j,galois_single_multiply( jm_inv->Get(i,j), tmp64_t, w ));
				*/
			}
		}

		/* element i,i == 1 now. */
		for( j = i + 1; j < rows; j++ ){

			r=j;
			c=i;
			elt = &(jm_cpy.Elts[(r*cols+c)/num_per_elt]);
			shift = bits_per_num*((r*cols+c)%num_per_elt);
			mask = new_elt_mask << shift;
			tmp64_t = ((*elt)&mask) >> shift;

			//tmp64_t = jm_cpy.Get(j,i);
			/* Every row below i is multiplied by row i times [j,i] so that
			   all entries in column i below row i = 0. */

			//get pointers to element j,k in jm_cpy and jm_inv
			r=j;
			c=0;
			elt = &(jm_cpy.Elts[(r*cols+c)/num_per_elt]);
			elt2 = &(jm_inv->Elts[(r*cols+c)/num_per_elt]);
			shift = bits_per_num*((r*cols+c)%num_per_elt);
			
			if( tmp64_t == 1 ){

				//get element i,k from both jm_cpy and jm_inv
				r=i;
				elt3 = &(jm_cpy.Elts[(r*cols+c)/num_per_elt]);
				elt4 = &(jm_inv->Elts[(r*cols+c)/num_per_elt]);
				shift2 = bits_per_num*((r*cols+c)%num_per_elt);
				mask = new_elt_mask << shift2;

				for( k = 0; k < cols; k++ ){

					//get i,k in jm_cpy
					tmp64_t = ((*elt3)&mask) >> shift2;
					//set jm_cpy's element j,k ^= element i,k
					(*elt) ^= (tmp64_t<<shift);

					//get i,k in jm_inv
					tmp64_t = ((*elt4)&mask) >> shift2;
					//set jm_inv's element j,k ^= element i,k
					(*elt2) ^= (tmp64_t<<shift);

					//setup to get the element in the next column
					mask <<= bits_per_num;
					shift2 += bits_per_num;
					if(mask==0){
						//new Elt for getting numbers in row i
						shift2 = 0;
						mask = new_elt_mask;
						elt3++;
						elt4++;
					}

					shift += bits_per_num;
					if(shift==64){
						//new Elt for getting numbers in row j
						shift = 0;
						elt++;
						elt2++;
					}
					/*
					The above loop is accomplishing:
					tmp64_t = jm_cpy.Get(j,k);
					jm_cpy.Set(j,k,tmp64_t^jm_cpy.Get(i,k));
					tmp64_t = jm_inv->Get(j,k);
					jm_inv->Set(j,k,tmp64_t^jm_inv->Get(i,k));
					*/
				}
			}else if(tmp64_t !=0){

				//get element i,k from both jm_cpy and jm_inv
				r=i;
				elt3 = &(jm_cpy.Elts[(r*cols+c)/num_per_elt]);
				elt4 = &(jm_inv->Elts[(r*cols+c)/num_per_elt]);
				shift2 = bits_per_num*((r*cols+c)%num_per_elt);
				mask = new_elt_mask << shift2;

				for( k = 0; k < cols; k++ ){

					//in both jm_cpy and jm_inv: {j,k} ^= galois_single_multiply(tmp64_t, {i,k},w)
					//get i,k in jm_cpy
					tmp64_t2 = mask32 & galois_single_multiply(tmp64_t,((*elt3)&mask) >> shift2,w);
					//tmp64_t2 = ((*elt3)&mask) >> shift2;
					//set jm_inv's element j,k ^= element i,k
					(*elt) ^= (tmp64_t2<<shift);

					//get i,k in jm_inv
					tmp64_t3 = mask32 & galois_single_multiply(tmp64_t,((*elt4)&mask) >> shift2,w);
					//tmp64_t3 = ((*elt4)&mask) >> shift2;
					//set jm_inv's element j,k ^= element i,k
					(*elt2) ^= (tmp64_t3<<shift);

					//setup to get the next number
					mask <<= bits_per_num;
					shift += bits_per_num;
					shift2 += bits_per_num;
					if(mask==0){
						shift2 = 0;
						mask = new_elt_mask;
						elt3++;
						elt4++;
					}

					if(shift == 64){
						shift = 0;
						elt++;
						elt2++;
					}

					/*
					The following is accomplishing:
					jm_cpy.Set(j,k,jm_cpy.Get(j,k)^galois_single_multiply(tmp64_t, jm_cpy.Get(i,k), w ));
					jm_inv->Set(j,k,jm_inv->Get(j,k)^galois_single_multiply(tmp64_t, jm_inv->Get(i,k), w ));
					*/
				}
			}
		}
	}

	/* Now the matrix is upper triangular.  Start at the top and multiply down  */
	for( i = rows - 1; i >= 0; i-- ){
		for( j = 0; j < i; j++ ){
			
			r=j;
			c=i;
			elt = &(jm_cpy.Elts[(r*cols+c)/num_per_elt]);
			shift = bits_per_num*((r*cols+c)%num_per_elt);
			mask = new_elt_mask << shift;
			tmp64_t = ((*elt)&mask);

			//tmp64_t = jm_cpy.Get(j,i);
			if( tmp64_t != 0 ){

				//get the actual value
				tmp64_t >>= shift;
				
				//get pointer to element j,k in jm_inv
				r=j;
				c=0;
				elt = &(jm_inv->Elts[(r*cols+c)/num_per_elt]);
				shift = bits_per_num*((r*cols+c)%num_per_elt);

				//get element i,k from jm_inv
				r=i;
				elt2 = &(jm_inv->Elts[(r*cols+c)/num_per_elt]);
				shift2 = bits_per_num*((r*cols+c)%num_per_elt);
				mask = new_elt_mask << shift2;

				for( k = 0; k < cols; k++ ){
					
					//set jm_inv's {j,k} ^= galois_single_mult(tmp64_t, jm_inv's {i,k},w)
					//get i,k in jm_inv
					tmp64_t2 = mask32 & galois_single_multiply(tmp64_t,((*elt2)&mask) >> shift2,w);
					//tmp64_t2 = ((*elt2)&mask) >> shift2;
					//set jm_inv's element j,k ^= element i,k
					(*elt) ^= (tmp64_t2<<shift);

					//setup to get the next number
					mask <<= bits_per_num;
					shift2 += bits_per_num;
					if(mask==0){
						shift2 = 0;
						mask = new_elt_mask;
						elt2++;
					}

					shift += bits_per_num;
					if(shift == 64){
						shift = 0;
						elt++;
					}
					//jm_inv->Set(j,k,jm_inv->Get(j,k)^galois_single_multiply( tmp64_t, jm_inv->Get(i,k), w ));
				}
			}
		}
	}

	//success
	return 0;
}
int invert_bitmatrix(JER_Matrix *jm, JER_Matrix *jm_inv){

	JER_Matrix jm_cpy;
	int i, j, k, w;
	int rows, cols;

	//variables for fast get/set
	uint64_t *elt, *elt2, *elt3, *elt4;
	uint64_t mask;
	char shift,shift2;
	int r,c;
	uint64_t *saved_elt; //saved elt and mask are used when we get the same r,c element multiple times
	uint64_t tmp64_t,tmp64_t2,tmp64_t3;
	uint64_t new_elt_mask;
	uint64_t saved_mask;

	new_elt_mask = 1;

	if(jm_inv == NULL){
		return -1;
	}

	if(jm == NULL){
		jm_inv->Elts.clear();
		jm_inv->R = 0;
		jm_inv->C = 0;
		return -1;	
	}

	rows = jm->R;
	cols = jm->C;
	w = jm->W;

	if(w != 1 || rows <= 0 || rows != cols){
		jm_inv->Elts.clear();
		jm_inv->R = 0;
		jm_inv->C = 0;
		return -1;	
	}

	//copy this matrix
	jm_cpy.Copy(jm);

	jm_inv->Create_Identity(rows,cols,w);

	for( i = 0; i < rows; i++ ){
		/* If there is a 0 on the diagonal on the ith row */
		r=i;
		c=i;
		saved_elt = &(jm_cpy.Elts[(r*cols+c)/64]);
		shift = ((r*cols+c)%64);
		saved_mask = new_elt_mask << shift;
		if( ((*saved_elt)&saved_mask) == 0 ){
		//if( jm_cpy.Get(i,i) == 0 )
			/* Go through each subsequent row looking for one with a non-zero
			   value in the ith column. When this row is moved to the ith row,
			   there will no longer be a 0 on the diagonal on the ith row. */
			j = i;
			do{
				j++;

				r=j;
				c=i;
				elt = &(jm_cpy.Elts[(r*cols+c)/64]);
				mask = new_elt_mask << ((r*cols+c)%64);
			}while(j< rows && ((*elt)&mask) == 0);
			//for( j = i + 1; j < rows && jm_cpy.Get(j,i) == 0; j++ );

			/* If j == rows, there was no row with a non-zero value in the ith
				 column below row i, therefore matrix is not invertible. */
			if( j == rows){
				jm_inv->Elts.clear();
				jm_inv->R = 0;
				jm_inv->C = 0;
				return -1;
			}
			/* Otherwise swap the ith and jth row. */
			jm_cpy.Swap_Rows(i,j);
			jm_inv->Swap_Rows(i,j);
		}

		/* element i,i == 1 now. */
		for( j = i + 1; j < rows; j++ ){

			r=j;
			c=i;
			elt = &(jm_cpy.Elts[(r*cols+c)/64]);
			shift = ((r*cols+c)%64);
			mask = new_elt_mask << shift;
			tmp64_t = ((*elt)&mask) >> shift;

			//tmp64_t = jm_cpy.Get(j,i);
			/* Every row below i is multiplied by row i times [j,i] so that
			   all entries in column i below row i = 0. */

			//get pointers to element j,k in jm_cpy and jm_inv
			r=j;
			c=0;
			elt = &(jm_cpy.Elts[(r*cols+c)/64]);
			elt2 = &(jm_inv->Elts[(r*cols+c)/64]);
			shift = ((r*cols+c)%64);
			
			if( tmp64_t == 1 ){

				//get element i,k from both jm_cpy and jm_inv
				r=i;
				elt3 = &(jm_cpy.Elts[(r*cols+c)/64]);
				elt4 = &(jm_inv->Elts[(r*cols+c)/64]);
				shift2 = ((r*cols+c)%64);
				mask = new_elt_mask << shift2;

				for( k = 0; k < cols; k++ ){

					//get i,k in jm_cpy
					tmp64_t = ((*elt3)&mask) >> shift2;
					//set jm_inv's element j,k ^= element i,k
					(*elt) ^= (tmp64_t<<shift);

					//get i,k in jm_inv
					tmp64_t = ((*elt4)&mask) >> shift2;
					//set jm_inv's element j,k ^= element i,k
					(*elt2) ^= (tmp64_t<<shift);

					//setup to get the next number
					mask <<= 1;
					shift2 += 1;
					if(mask==0){
						shift2 = 0;
						mask = new_elt_mask;
						elt3++;
						elt4++;
					}
				
					shift += 1;
					if(shift == 64){
						shift = 0;
						elt++;
						elt2++;
					}

					/*
					tmp64_t = jm_cpy.Get(j,k);
					jm_cpy.Set(j,k,tmp64_t^jm_cpy.Get(i,k));
					tmp64_t = jm_inv->Get(j,k);
					jm_inv->Set(j,k,tmp64_t^jm_inv->Get(i,k));
					*/
				}
			}
		}

	}

	/* Now the matrix is upper triangular.  Start at the top and multiply down  */
	for( i = rows - 1; i >= 0; i-- )
	{
		for( j = 0; j < i; j++ )
		{
			
			r=j;
			c=i;
			elt = &(jm_cpy.Elts[(r*cols+c)/64]);
			shift = ((r*cols+c)%64);
			mask = new_elt_mask << shift;
			tmp64_t = ((*elt)&mask);

			//tmp64_t = jm_cpy.Get(j,i);
			if( tmp64_t != 0 )
			{
				//get the actual value
				tmp64_t >>= shift;
				
				//get pointer to element j,k in jm_inv
				r=j;
				c=0;
				elt = &(jm_inv->Elts[(r*cols+c)/64]);
				shift = ((r*cols+c)%64);

				//get element i,k from jm_inv
				r=i;
				elt2 = &(jm_inv->Elts[(r*cols+c)/64]);
				shift2 = ((r*cols+c)%64);
				mask = new_elt_mask << shift2;

				for( k = 0; k < cols; k++ )
				{
					
					//set jm_inv's {j,k} ^= galois_single_mult(tmp64_t, jm_inv's {i,k},w)
					//get i,k in jm_inv
					//tmp64_t2 = galois_single_multiply(tmp64_t,((*elt2)&mask) >> shift2,w);
					tmp64_t2 = ((*elt2)&mask) >> shift2;
					//set jm_inv's element j,k ^= element i,k
					(*elt) ^= (tmp64_t2<<shift);

					//setup to get the next number
					mask <<= 1;
					shift2 += 1;
					if(mask==0){
						shift2 = 0;
						mask = new_elt_mask;
						elt2++;
					}

					shift += 1;
					if(shift == 64){
						shift = 0;
						elt++;
					}

					//jm_inv->Set(j,k,jm_inv->Get(j,k)^galois_single_multiply( tmp64_t, jm_inv->Get(i,k), w ));
				}
			}
		}
	}
	
	//success
	return 0;
}

/* Takes inverse of m and return it or stores in inv if specified. */
JER_Matrix *Inverse( JER_Matrix *m ){

	JER_Matrix* m_inv;
	int r;
	int w;

	if(m == NULL){
		return NULL;
	}

	m_inv = new JER_Matrix;

	w = m->W;
	
	if(w == 1){
		r = invert_bitmatrix(m,m_inv);
	}else{
		r = invert_matrix(m,m_inv);
	}

	if(r == 0){
		//success
		return m_inv;
	}else{
		//not invertible
		delete m_inv;
		return NULL;
	}

}

JER_Matrix Inverse( JER_Matrix &m ){

	JER_Matrix m_inv;
	int r;
	int w;

	w = m.W;
	
	if(w == 1){
		r = invert_bitmatrix(&m,&m_inv);
	}else{
		r = invert_matrix(&m,&m_inv);
	}

	if(r == 0){
		//success
		return m_inv;
	}else{
		//m_inv was resized to size 0
		//when the invert procedure failed
		return m_inv;
	}

}
int Inverse( JER_Matrix *m, JER_Matrix *inv){

	int r;
	int w;

	if(inv == NULL){
		return -1;
	}

	if(m==NULL){
		inv->R=0;
		inv->C=0;
		inv->Elts.clear();
	}

	w = m->W;

	if(w == 1){
		r = invert_bitmatrix(m,inv);
	}else{
		r = invert_matrix(m,inv);
	}

	if(r == 0){
		//success
		return 0;
	}else{
		//not invertible
		return -1;
	}

}
int Inverse( JER_Matrix &m, JER_Matrix &inv ){

	int r;
	int w;

	w = m.W;

	if(w == 1){
		r = invert_bitmatrix(&m,&inv);
	}else{
		r = invert_matrix(&m,&inv);
	}

	if(r == 0){
		//success
		return 0;
	}else{
		//not invertible
		return -1;
	}

}

/* Converts a matrix to bitmatrix */
JER_Matrix *JER_Matrix::Matrix_To_Bitmatrix()
{
	int i,j,l,x;
	uint64_t elt;

	JER_Matrix * jbm;

	if(R == 0 || C == 0){
		return NULL;
	}

	jbm = new JER_Matrix(R*W,C*W,1);
	if(jbm == NULL){
		return NULL;
	}
	/* Copied more or less from original. */
	for( i = 0; i < R; i++ )
	{
		for( j = 0; j < C; j++ )
		{
			/* Take the value at matrix[i][j] and store it as it bitmatrix
				 specification. */
			elt = Get(i,j);
			for( x = 0; x < W; x++ )
			{
				/* Set the bit in the xth row of the ith super row. */
				for( l = 0; l < W; l++ )
				{
					/* Set the lth bits of the jth super col to the bit rep
						 of elt. */
					jbm->Set( i*W+l, j*W+x, ((elt & (1 << l)) ? 1 : 0) );
				}
				elt = (unsigned int) galois_single_multiply( elt, 2, W );
			}
		}
	}

	return jbm;	
}

JER_Matrix *JER_Matrix::Bitmatrix_To_Matrix( int WPD )
{
	JER_Matrix *result;
	int i, j, k;
	uint64_t val;
	
	if( R < WPD || C < WPD || R % WPD != 0 || C % WPD != 0 )
		return NULL;

	result = new JER_Matrix( R / WPD, C / WPD, WPD );
	if( result == NULL )
		return NULL;

	for( i = 0; i < result->R; i ++ )
	{
		for( j = 0; j < result->C; j++ )
		{
			val = 0;
			for( k = 0; k < WPD; k++ )
			{
				if(Get(i*WPD+k,j*WPD)==1){
					val += 1<<k;
				}
			}
			result->Set( i, j, val);
		}
	}
	return result;
}

int JER_Matrix::Bitmatrix_To_Matrix( JER_Matrix &jm, int WPD )
{
	int i, j, k;
	uint64_t val;
	if( R < WPD || C < WPD || R % WPD != 0 || C % WPD != 0 )
		return -1;

	if( jm.Create_Empty( R / WPD, C / WPD, WPD ) == -1 )
		return -1;

	for( i = 0; i < jm.R; i++ )
	{
		for( j = 0; j < jm.C; j++ )
		{
			val = 0;
			for( k = 0; k < WPD; k++ )
			{
				if(Get(i*WPD+k,j*WPD)==1){
					val += 1<<k;
				}
			}
			jm.Set( i, j, val);
		}
	}
	return 0;
}
	
int JER_Matrix::Matrix_To_Bitmatrix( JER_Matrix & jbm){

	int i,j,l,x;
	uint64_t elt;

	if(R == 0 || C == 0){
		return -1;
	}

	if(jbm.Create_Empty(R*W,C*W,1) == -1){
		return -1;
	}

	/* Copied more or less from original. */
	for( i = 0; i < R; i++ )
	{
		for( j = 0; j < C; j++ )
		{
			/* Take the value at matrix[i][j] and store it as it bitmatrix
				 specification. */
			elt = Get(i,j);
			for( x = 0; x < W; x++ )
			{
				/* Set the bit in the xth row of the ith super row. */
				for( l = 0; l < W; l++ )
				{
					/* Set the lth bits of the jth super col to the bit rep
						 of elt. */
					jbm.Set( i*W+l, j*W+x, ((elt & (1 << l)) ? 1 : 0) );
				}
				elt = (unsigned int) galois_single_multiply( elt, 2, W );
			}
		}
	}

	return 0;	
}

JER_Schedule *JER_Matrix::To_Schedule_CSHR(int wpd){

	JER_Schedule * sched;
	int r,c;
	int rows,cols;
	int optodo;
	vector <unsigned int> v5; //5 element vector
	int *diff, *from, *flink, *blink;
	int i, j;
	int no,row;
	int op;
	int bestrow, bestdiff, top;

	rows = R;
	if(!rows) return NULL;

	cols = C;
	if(!cols) return NULL;

	if( W != 1 ) return NULL;

	sched = new JER_Schedule();
	if( sched == NULL )
		return NULL;

	diff = talloc(int, rows);
	from = talloc(int, rows);
	flink = talloc(int, rows);
	blink = talloc(int, rows);
	if( diff == NULL || from == NULL || flink == NULL || blink == NULL )
		return NULL;
	bestdiff = cols+1;
	top = 0;
	op =0;
	for (i = 0; i < rows; i++) {
		no = 0;
		/* Count number of 1s in row i. Store in diff[i]. */
		for (j = 0; j < cols; j++) {
			no += Get(i,j);
		}
		diff[i] = no;
		from[i] = -1;
		flink[i] = i+1;
		blink[i] = i-1;
		/* Find row with least number of 1s. */
		if (no < bestdiff) {
			bestdiff = no;
			bestrow = i;
		}
	}

	flink[rows-1] = -1;
	while (top != -1) {
		row = bestrow;

		/* Pull row out of double-linked list so that it may be processed. */
		if (blink[row] == -1) {
			top = flink[row];
			if (top != -1) blink[top] = -1;
		} else {
			flink[blink[row]] = flink[row];
			if (flink[row] != -1) {
				blink[flink[row]] = blink[row];
			}
		}


		/* Row does not benefit from having dependencies, simply generate it
		   dumb. */
		if (from[row] == -1) {
			optodo = 0;
			for (j = 0; j < cols; j++) {
				if (Get(row,j)) {
					sched->Elts.push_back(v5);
					sched->Elts[op].resize(5);
					sched->Elts[op][4] = optodo;
					sched->Elts[op][0] = j/wpd;
					sched->Elts[op][1] = j%wpd;
					sched->Elts[op][2] = C/wpd+row/wpd;
					sched->Elts[op][3] = row%wpd;
					op++;
					optodo = 1;
				}
			}
		}
		/* Otherwise generate it starting from its dependency. 
		   The first operation is a copy of from[row]; each subsequent action
		   is xor with individual bits. */
		else {
			sched->Elts.push_back(v5);
			sched->Elts[op].resize(5);
			sched->Elts[op][4] = 0;
			sched->Elts[op][0] = C/wpd + from[row]/wpd;
			sched->Elts[op][1] = from[row]%wpd;
			sched->Elts[op][2] = C/wpd+row/wpd;
			sched->Elts[op][3] = row%wpd;
			op++;

			for (j = 0; j < cols; j++) {
				if (Get(row,j) ^ Get(from[row],j)) {
					sched->Elts.push_back(v5);
					sched->Elts[op].resize(5);
					sched->Elts[op][4] = 1;
					sched->Elts[op][0] = j/wpd;
					sched->Elts[op][1] = j%wpd;
					sched->Elts[op][2] = C/wpd+row/wpd;
					sched->Elts[op][3] = row%wpd;
					op++;
					optodo = 1;
				}
			}
		}
		bestdiff = cols+1;
		/* Look for next smallest row. Before checking diff[i] for min, first
		   see if diff[i] reduced by xoring the recently processed row with 
		   row i. */
		for (i = top; i != -1; i = flink[i]) {
			/* Operations for using previous entry = 1 + number of 1s in xor of
			   two rows. */
			no = 1;
			for (j = 0; j < cols; j++) no += (Get(row,j) ^ Get(i,j));
			if (no < diff[i]) {
				from[i] = row;
				diff[i] = no;
			}
			if (diff[i] < bestdiff) {
				bestdiff = diff[i];
				bestrow = i;
			}
		}
	}

	free(from);
	free(diff);
	free(blink);
	free(flink);

	return sched;
}

JER_Schedule *JER_Matrix::To_Schedule_Dumb(int wpd){

	JER_Schedule * sched;
	int r,c;
	int rows,cols;
	int optodo,op;
	vector < unsigned int > v5;

	rows = R;
	if(!rows) return NULL;

	cols = C;
	if(!cols) return NULL;

	if( W != 1 ) return NULL;

	sched = new JER_Schedule();
	if( sched == NULL )
		return NULL;

	op = 0;
	for(r=0;r<rows;r++){
		optodo = 0;
		for(c=0;c<cols;c++){
			/* If jbm[r][c] == 1, xor (copy if its the first time) device c/w 
			   packet c%w into device k + (r/w) packet r%w. */
			if(Get(r,c)){
				sched->Elts.push_back(v5);
				sched->Elts[op].resize(5);
				sched->Elts[op][4] = optodo;
				sched->Elts[op][0] = c/wpd;
				sched->Elts[op][1] = c%wpd;
				sched->Elts[op][2] = C/wpd+r/wpd;
				sched->Elts[op][3] = r%wpd;
				optodo = 1;
				op++;
			}
		}
	}

	/* last schedules' 0th element (source disk) no longer needs to be -1, 
	   because with vectors we can just use size() */
	return sched;
}

//Private member:
int JER_Matrix::Resize_Elts()
{
	try
	{
		if( W == 1 )
			Elts.resize( ( (R*C) - 1 ) / 64 + 1, 0 );
		else if( W <= 8 )
			Elts.resize( ( (R*C) - 1 ) / 8 + 1, 0 );
		else if( W <= 16 )
			Elts.resize( ( (R*C) - 1 ) / 4 + 1, 0 );
		else if( W <= 32 )
			Elts.resize( ( (R*C) - 1 ) / 2 + 1, 0 );
		else
			Elts.resize( R * C, 0 );
	}
	catch(...){ return -1; }
	return 0;
}
